#pragma once
// 是否打印数据
#include "../src/headfile.h"
struct Lidar_init;
/***********************************模板函数的实现***********************************/
template <typename T, typename T2>
void Myplus(std::vector<T> &p, int maxSize, T2 i, T para);

template <typename T>
double calculateAverage(const std::vector<T> &radii);
/**
 * @brief 计算两个激光点之间的欧几里得距离。
 *
 * @param a 第一个激光点。
 * @param b 第二个激光点。
 * @return 点 a 和 b 之间的欧几里得距离。
 */
template <typename T, typename T2>
double Euclidean_distance(T &a, T2 &b)
{
    return sqrt(a.y * a.y + b.y * b.y - 2 * a.y * b.y * cos((b.x - a.x) * (M_PI / 180.0)));
}

/**
 * @brief 针对容器可动态分配大小的特性 将一定size数据存储并实时替换旧数据
 *
 * @param vector<T> &p      需要存储的容器
 * @param int maxSize         设置存储最大尺寸
 * @param T2 i                          容器的索引
 * @param T para                    存储的数据
 * @return 无返回值
 *
 */
template <typename T, typename T2>
void Myplus(std::vector<T> &p, int maxSize, T2 i, T para)
{
    if (p.size() == maxSize)
    {
        p[i] = para;
    }
    else
    {
        p.push_back(para);
    }
}

// 计算容器的平均值
template <typename T>
double calculateAverage(const std::vector<T> &radii)
{
    double sum = accumulate(radii.begin(), radii.end(), 0.0);
    return sum / radii.size(); // 平均曲率为所有点的曲率之和除以点的数量
}

// 对特征数据进行高斯归一化
template <typename T>
void Gaaosi_F_P(T &features, const int Feature_Nums)
{
    double sum = 0;
    // 将结构体中的变量进行求和
    for (int i = 0; i < Feature_Nums; i++)
    {
        sum += getFeature(features, i);
    }
    double mean = sum / Feature_Nums; // 计算均值
    double pow_diff_mean = 0;
    for (int i = 0; i < Feature_Nums; i++)
    {
        pow_diff_mean += pow(getFeature(features, i) - mean, 2);
    }
    pow_diff_mean = sqrt(pow_diff_mean / Feature_Nums);
    features.num_Points = (features.num_Points - mean) / (pow_diff_mean);
    features.average_angleDiff = (features.average_angleDiff - mean) / (pow_diff_mean);
    features.radius = (features.radius - mean) / (pow_diff_mean);
    features.arc_length = (features.arc_length - mean) / (pow_diff_mean);
    features.average_widthDiff = (features.average_widthDiff - mean) / (pow_diff_mean);
    features.average_curvature = (features.average_curvature - mean) / (pow_diff_mean);
    features.sum_Point2Center = (features.sum_Point2Center - mean) / (pow_diff_mean);
    features.xPoint2Center_mean = (features.xPoint2Center_mean - mean) / (pow_diff_mean);
    features.yPoint2Center_mean = (features.yPoint2Center_mean - mean) / (pow_diff_mean);
    features.xPoint2SortX = (features.xPoint2SortX - mean) / (pow_diff_mean);
    features.yPoint2SortY = (features.yPoint2SortY - mean) / (pow_diff_mean);
    features.xPoint2averageX = (features.xPoint2averageX - mean) / (pow_diff_mean);
    features.yPoint2averageY = (features.yPoint2averageY - mean) / (pow_diff_mean);
    features.xKurtosis = (features.xKurtosis - mean) / (pow_diff_mean);
    features.yKurtosis = (features.yKurtosis - mean) / (pow_diff_mean);
    features.xSkewness = (features.xSkewness - mean) / (pow_diff_mean);
    features.ySkewness = (features.ySkewness - mean) / (pow_diff_mean);
    features.cen_x_zero_sum = (features.cen_x_zero_sum - mean) / (pow_diff_mean);
    features.zero_x_cen_sum = (features.zero_x_cen_sum - mean) / (pow_diff_mean);
    features.cen_y_zero_sum = (features.cen_y_zero_sum - mean) / (pow_diff_mean);
    features.zero_y_cen_sum = (features.zero_y_cen_sum - mean) / (pow_diff_mean);
    features.cen_x_sum = (features.cen_x_sum - mean) / (pow_diff_mean);
    features.x_cen_sum = (features.x_cen_sum - mean) / (pow_diff_mean);
    features.cen_y_sum = (features.cen_y_sum - mean) / (pow_diff_mean);
    features.y_cen_sum = (features.y_cen_sum - mean) / (pow_diff_mean);
}

template <typename T>
double getFeature(T &features, int index)
{
    switch (index)
    {
    case 0:
        return features.num_Points;
        break;
    case 1:
        return features.average_angleDiff;
        break;
    case 2:
        return features.radius;
        break;
    case 3:
        return features.arc_length;
        break;
    case 4:
        return features.average_widthDiff;
        break;
    case 5:
        return features.average_curvature;
        break;
    case 6:
        return features.sum_Point2Center;
        break;
    case 7:
        return features.xPoint2Center_mean;
        break;
    case 8:
        return features.yPoint2Center_mean;
        break;
    case 9:
        return features.xPoint2SortX;
        break;
    case 10:
        return features.yPoint2SortY;
        break;
    case 11:
        return features.xPoint2averageX;
        break;
    case 12:
        return features.yPoint2averageY;
        break;
    case 13:
        return features.xKurtosis;
        break;
    case 14:
        return features.yKurtosis;
        break;
    case 15:
        return features.xSkewness;
        break;
    case 16:
        return features.ySkewness;
        break;
    case 17:
        return features.cen_x_zero_sum;
        break;
    case 18:
        return features.zero_x_cen_sum;
        break;
    case 19:
        return features.cen_y_zero_sum;
        break;
    case 20:
        return features.zero_y_cen_sum;
        break;
    case 21:
        return features.x_cen_sum;
        break;
    case 22:
        return features.cen_x_sum;
        break;
    case 23:
        return features.y_cen_sum;
        break;
    case 24:
        return features.cen_y_sum;
        break;
    default:
        return 0;
        break;
    }
}
/***********************************非模板函数的声明***********************************/
double calculateAngle(double x1, double y1, double x2, double y2);

// 计算两点之间的距离
double calculateDistance(double x1, double y1, double x2, double y2);

// 计算曲率
double calculateCurvature(double radius);

// 计算区域峰度
double calculateKurtosis(const std::vector<double> &sum1, const std::vector<double> &sum2);

/*
雷达数据转换为X，Y坐标
*/
double Axis_x(double angle, double distance);
double Axis_y(double angle, double distance);
